/**
 * Snowflake MEGA sumo robot strategy nr.0 - attack straight, full speed forward
 * Copyright 2013 Maris BECS <maris.becs@gmail.com>
 */

#include <asf.h>
#include "strategy0.h"
#include "keyboard.h"
#include "sensors.h"
#include "motors.h"


void strategy0_app(void)
{
    struct keyboard_event input;

    // set speed for the strategy
    int8_t speed = 100;
    int8_t frc = -10;    // forward speed correction for the right motor


    // clear screen
    gfx_mono_draw_filled_rect(0, 0, 128, 32, GFX_PIXEL_CLR);
    // Show strategy info
    gfx_mono_draw_string("Strategy 0   STRAIGHT", 0, 0, &sysfont);
    gfx_mono_draw_string(" Press ENTER/IR start", 0, 24, &sysfont);

    while (true) {
        /*/ wait for IR remote start signal or ENTER button press
        if ( HIGH == ioport_get_pin_level(REMOTE_START) ){
            break;
        }*/
        keyboard_get_key_state(&input);
        if (input.type == KEYBOARD_RELEASE) {
            if (input.keycode == KEYBOARD_ENTER) {
                break;
            } else if (input.keycode == KEYBOARD_BACK) {
                // set IR module back to OFF state
                ioport_set_pin_high(START_SW);
                // go back to FIGHT menu
                return;
            }
        }
    }

    // ~5 sec delay before a start ( 950*5 = 4.75 sec )
    gfx_mono_draw_string("       5 sec to start", 0, 24, &sysfont);
    gpio_set_pin_low(NHD_C12832A1Z_BACKLIGHT);
        delay_ms(950);
    gpio_set_pin_high(NHD_C12832A1Z_BACKLIGHT);
        delay_ms(950);
    gpio_set_pin_low(NHD_C12832A1Z_BACKLIGHT);
        delay_ms(950);
    gpio_set_pin_high(NHD_C12832A1Z_BACKLIGHT);
        delay_ms(950);
    gpio_set_pin_low(NHD_C12832A1Z_BACKLIGHT);
        delay_ms(950);
    gfx_mono_draw_string("                &    ", 0, 12, &sysfont);
    gfx_mono_draw_string("      RUNNING =)     ", 0, 24, &sysfont);
    gpio_set_pin_high(NHD_C12832A1Z_BACKLIGHT);


    // drop the flag
    turn_servo(true);

    // turn on power for the sensors
    sensorPower(true);

    while (true)
    {
        // check kill switch state
        if ( LOW == ioport_get_pin_level(KILL_SW) ) {
            // kill switch has been activated
            drive(0,0);
            // set IR module back to OFF state
            ioport_set_pin_high(START_SW);
            // turn off power for sensors
            sensorPower(false);
            pwm_start( &pwm_cfg[1], 0 ); // free the servo
            // clear screen and turn on back-light
            gfx_mono_draw_filled_rect(0, 0, 128, 32, GFX_PIXEL_CLR);
            gpio_set_pin_high(NHD_C12832A1Z_BACKLIGHT);
            delay_ms(100);
            stop_motors();  // free motors
            return;
        }

        // while loop sensor read delay
        delay_ms(3);

        // read all sensors
        Line_sens = line_read();
        Distance_sens = sensor_read();

        // drive straight with right motor correction !!!
        drive( speed, speed+frc );


        /*
         * ######################### Check line sensor state #########################
         */
        if( (Line_sens&0b00000010) || (Line_sens&0b00000100) ){    // Line_FL or Line_FR
            delay_us(500);
            Line_sens = line_read();
            // Line_FL
            if (Line_sens&0b00000010) {
                drive(0,0);    // stop
                delay_ms(50);
                // turn right while NOT see line again
                while ( !(Line_sens&0b00000001) ){    // Line_BR
                    Line_sens = line_read();
                    drive(speed,-speed);
                    // check kill switch state
                    if ( LOW == ioport_get_pin_level(KILL_SW) ) {
                        drive(0,0);
                        delay_ms(100);
                        stop_motors();  // free motors
                        break;
                    }
                }
            // Line_FR
            } else if (Line_sens&0b00000100) {
                drive(0,0);    // stop
                delay_ms(50);
                // turn left while NOT see line again
                while ( !(Line_sens&0b00001000) ){    // Line_BL
                    Line_sens = line_read();
                    drive(-speed,speed);
                    // check kill switch state
                    if ( LOW == ioport_get_pin_level(KILL_SW) ) {
                        drive(0,0);
                        delay_ms(100);
                        stop_motors();  // free motors
                        break;
                    }
                }
            }
        }


        /*
         * ######################### Check distance sensor state #########################
         */
        if (Distance_sens&0b00010000) {        // L10
            drive( speed*0.7, speed );    // drive little left
            delay_ms(5);

        } else if (Distance_sens&0b00001000) {    // R10
            drive( speed, speed*0.7 );    // drive little right
            delay_ms(5);

        } else if (Distance_sens&0b00100000) {    // L30
            drive( speed/3, speed );    // drive more left
            delay_ms(5);

        } else if (Distance_sens&0b00000100) {    // R30
            drive( speed, speed/3 );    // drive more right
            delay_ms(5);

        } else if (Distance_sens&0b01000000) {    // L45
            drive( -speed, speed );    // turn on spot left
            delay_ms(5);

        } else if (Distance_sens&0b00000010) {    // R45
            drive( speed, -speed );    // turn on spot right
            delay_ms(5);

        } else if (Distance_sens&0b10000000) {    // L90
            drive( -speed, speed );    // turn on spot left
            delay_ms(5);

        } else if (Distance_sens&0b00000001) {    // R90
            drive( speed, -speed );    // turn on spot right
            delay_ms(5);
        } /*else if ( HIGH == ioport_get_pin_level(PEPPERL_FUCHS) ){
            drive( speed, speed+frc );    // drive straight full speed, not sure if necessary !!!
            delay_ms(5);
        }*/


    }
}